/*
Work in this file belongs to DOTTED LINE FOLLOWING group. 
Other groups are not allowed to make changes on this file. 

During development, you can use your own test code that
includes your own setup() and loop() functions but they 
should not be included when you are submitting your changes. 

Put your own code here. Example:

void someFunction(int someParameter)
{
  
}

*/


void dlLineFollow(){
  
  dlMeasurement = 0;
  dlCount = 0; //number of sensors on the line

  //read sensors and give waight
  if (digitalRead(12)) dlSensors[0] = 0;
  else dlSensors[0] = 1;
  if (digitalRead(11)) dlSensors[1] = 0;
  else dlSensors[1] = 1;
  if (digitalRead(10)) dlSensors[2] = 0;
  else dlSensors[2] = 1;
  if (digitalRead(9)) dlSensors[3] = 0;
  else dlSensors[3] = 1;
  if (digitalRead(8)) dlSensors[4] = 0;
  else dlSensors[4] = 1;
  
  //calculate count, number of on sensors
  int i;
  for (i=0; i<=4; i++){
    if (dlSensors[i])  dlCount++;
  }
  
  if (dlCount != 0){
   dlLeft = 0;
   dlRight = 0; 
  }
  else {
   dlCount++; 
  }
  
  //measurement is value that where the line stands under car
  dlMeasurement = (1*dlLeft) + (4*dlSensors[0]) + (6*dlSensors[1]) + (8*dlSensors[2]) + (10*dlSensors[3]) + (12*dlSensors[4]) + (15*dlRight);
  dlMeasurement = dlMeasurement / dlCount;
  
  //calculating the error
  if (dlMeasurement != 0) dlError = dlMeasurement - dlSetpoint;
  else dlError = 0; //error for proportional 
  
  //time interval for calculating Derivative  
  unsigned long dlNow = millis();
  int dlTimeChange = dlNow - dlLastTime;
    
  dlDErr = (dlError - dlLastError); //error for derivative
  dlProportional = dlKp*dlError; //propotional calculation
    
    
  if(dlTimeChange>=dlSampleTime){
    dlDerivative = dlKd*dlDErr; //derivative calculation
    
    /*
    Serial.print(proportional);
    Serial.print(" ");
    Serial.println(derivative);
    */

    dlLastError = dlError;
    dlLastTime = dlNow;
  }
 
  dlTurn = dlProportional + dlDerivative; //can take (+), (-) values, stores the value for motorPower
  dlPowerLeft = (dlMp + dlTurn) > 255 ? 255 : dlMp + dlTurn; //left motor power
  dlPowerRight = (dlMp - dlTurn) < 0 ? 0 : dlMp - dlTurn; //right motor power
  
  advance(dlPowerLeft, dlPowerRight); //motor grubunun yazdığı fonksiyon
  
  //when the car out of the line, remember where was the line
  if (dlSensors[4]) {
    dlRight = 1;
  }
  else if (dlSensors[0]) {
    dlLeft = 1;
  }
  //delay(5);
}





void dlDottedLineFollow(){
  
  advance(150, 150);
  
  dlLastSpeed[RIGHT] = dlCoder[RIGHT];
  
  dlCoder[RIGHT] = 0;
  
  dlTime2 = dlTime;

  if(dlTime2>dlTime1){
    dlTime3 = dlTime2 - dlTime1;
  }
  
  dlNewX = dlCount * dlDistancebetweenline;
  
  dlTime1 = dlTime2; 
  
}

void dlLwheelSpeed()
{
  dlTime = millis();
  Serial.print("giris: "); Serial.print(dlTime1);Serial.print("cikis: "); Serial.print(dlTime2); Serial.print("fark: ");Serial.println(dlTime3);Serial.print("x: ");Serial.println(dlNewX);
  dlCount++;
}
 
 
void dlRwheelSpeed()
{
  dlCoder[RIGHT] ++; //count the right wheel encoder interrupts
}
